#include <base\rotation.h>
#include <base\matrix.h>

#include <cmath>

rotation::rotation() : quat(0.0, 0.0, 0.0, 1.0) {
}

rotation::rotation(const vec3f & axis, const float radians) {
  quat[3] = static_cast<float>(cos(radians/2));

  const float sineval = static_cast<float>(sin(radians/2));
  vec3f a = axis;

  // we test for a null vector above
  (void) a.normalize();
  quat[0] = a[0] * sineval;
  quat[1] = a[1] * sineval;
  quat[2] = a[2] * sineval;
}

rotation::rotation(const float q0, const float q1, const float q2, const float q3) {
  quat.x = q0;
  quat.y = q1;
  quat.z = q2;
  quat.w = q3;
}

rotation 
rotation::identity(void) {
  return rotation(0.0f, 0.0f, 0.0f, 1.0f);
}

void
rotation::getValue(matrix & matrix_) const {
  float l = this->quat.length();
  float x,y,z,w;
  if (l > FLT_EPSILON) {
    // normalize it
    x = quat.x / l;
    y = quat.y / l;
    z = quat.z / l;
    w = quat.w / l;
  }
  else {
   // identity
    x = y = z = 0.0f;
    w = 1.0f;
  }

  matrix_[0][0] = 1.0f - 2.0f * (y * y + z * z);
  matrix_[0][1] = 2.0f * (x * y + z * w);
  matrix_[0][2] = 2.0f * (z * x - y * w);
  matrix_[0][3] = 0.0f;

  matrix_[1][0] = 2.0f * (x * y - z * w);
  matrix_[1][1] = 1.0f - 2.0f * (z * z + x * x);
  matrix_[1][2] = 2.0f * (y * z + x * w);
  matrix_[1][3] = 0;

  matrix_[2][0] = 2.0f * (z * x + y * w);
  matrix_[2][1] = 2.0f * (y * z - x * w);
  matrix_[2][2] = 1.0f - 2.0f * (y * y + x * x);
  matrix_[2][3] = 0.0f;

  matrix_[3][0] = 0.0f;
  matrix_[3][1] = 0.0f;
  matrix_[3][2] = 0.0f;
  matrix_[3][3] = 1.0f;
}

const float *
rotation::getValue(void) const {
  return &this->quat.x;
}